% FILE: extract_ulog.m
% DESCRIPTION:This demo file is used for read PX4-Autopilot`s .ulog files and convert
% to matlab .mat files.
% HINT: Required for Matlab UAV Toolbox, see
% https://www.mathworks.com/help/uav/ref/ulogreader.html.
% If you have ulog read issue, see 
% https://se.mathworks.com/matlabcentral/answers/2029804-ulogreader-no-longer-works-with-new-px4-logs

clc, clear, close all

% Load ulog data from file.
SAVE_MAT = 0;
PLOT = 0;

file_name = "ulog";
topic_name = "rate_chirp_sweep";

ulog = ulogreader(file_name + '.ulg');

log_duration = seconds(ulog.EndTime - ulog.StartTime);
disp("Log file name:" + file_name + ", duration:" + log_duration + '.');

d1 = ulog.StartTime + duration([0 0 0],'Format','hh:mm:ss.SSSSSS');
d2 = ulog.StartTime + duration([0 0 log_duration],'Format','hh:mm:ss.SSSSSS');

% Read topic message into a shell
topic_cell = readTopicMsgs(ulog,'TopicNames',{topic_name}, 'InstanceID',{0},'Time',[d1 d2]);
topic = topic_cell.TopicMessages{1,1};

t = seconds(topic.timestamp);
t = t - t(1,1);

chirp = topic.chirp(:,1); 
r = topic.r(:,1);
u = topic.u(:,1);
y = topic.y(:,1);

% Plot (optional)
if PLOT
    fig=figure;
    
    subplot(4,1,1);
    plot(t, chirp);
    ylabel('chirp');
    
    subplot(4,1,2);
    plot(t, r);
    ylabel('r');
    
    subplot(4,1,3);
    plot(t, u);
    ylabel('u');
    
    subplot(4,1,4);
    plot(t, y);
    xlabel('time(s)');
    ylabel('y');
end

% Save .mat files (optional)
if SAVE_MAT
    clearvars -except t chirp r u y file_name topic_name;
    save_mat_filename = file_name + "_" + topic_name + "_result.mat";
    save(save_mat_filename);
    disp("Saved mat file as:" + '[' + save_mat_filename + ']' + '.');
end

